#include <PID_v1.h>
#include <Ticker.h>
Ticker timer_read_encoder;  // 中断函数定时器定义,声明ticker对象

int32_t Velocity_L, Velocity_R;   //左右轮编码器数据
double Velocity_Left, Velocity_Right;     //左右轮速度

void READ_ENCODER_L(void);
void READ_ENCODER_R(void);
void read(void);

#define ENCODER_L 	12 //编码器采集引脚 每路2个 共4个
#define DIRECTION_L 13
#define ENCODER_R   27
#define DIRECTION_R 26
#define interrupt_time 100 // 中断时间

double gv_pid_speed_r, gv_pid_speed_set_r, gv_pid_speed_out_r;
double gv_pid_speed_l, gv_pid_speed_set_l, gv_pid_speed_out_l;

//Specify the links and initial tuning parameters
double Kp = 0.4;
double Ki = 0.3;
double Kd = 0;

PID myPID_r(&Velocity_Left, &gv_pid_speed_out_r, &gv_pid_speed_set_r, Kp, Ki, Kd, DIRECT);

void init_pid() {
  //turn the PID on
//   myPID_r.SetOutputLimits(0,255);
  myPID_r.SetSampleTime(20);

  myPID_r.SetMode(AUTOMATIC);
  // myPID_l.SetMode(AUTOMATIC);
}


void check_pid() {
  myPID_r.Compute();
  // myPID_l.Compute();
}

void set_speed_l( double iv_speed ) {
  int lv_speed = map(iv_speed, 0, 100, 0, 255);
  lv_speed = constrain(lv_speed, 0, 255);
  digitalWrite(AIN1_PIN, LOW);
  digitalWrite(AIN2_PIN, HIGH);
  analogWrite(PWMA_PIN, lv_speed);

}


void set_speed_r( int iv_speed, short iv_dir ) {
  int lv_speed = map(iv_speed, 0, 30, 0, 255);
  lv_speed = constrain(lv_speed, 0, 255);
  digitalWrite(BIN1_PIN, LOW);
  digitalWrite(BIN2_PIN, HIGH);
  analogWrite(PWMA_PIN, lv_speed);
}

/*********************
速度环PI：Kp*Ek+Ki*Ek_S
*********************/
// void speed_pid(Speed_PID *Speed_pid)
// {
// 	//1.计算速度偏差
// 	Speed_pid->err=(Speed_pid->moto_left+Speed_pid->moto_right)-Speed_pid->target;
// 	//2.对速度偏差进行低通滤波
// 	//使得波形更加平滑，滤除高频干扰，防止速度突变。
// 	Speed_pid->filter_err=(1-Speed_pid->filter_a)*Speed_pid->err+Speed_pid->filter_a*Speed_pid->filter_err_last;
	
// 	Speed_pid->filter_err_last=Speed_pid->filter_err;
// 	//3.对速度偏差积分，积分出位移
// 	Speed_pid->err_integral+=Speed_pid->filter_err;
// 	//4.积分限幅	
// 	Speed_pid->err_integral=Speed_pid->err_integral>10000?10000:(Speed_pid->err_integral<(-10000)?(-10000):Speed_pid->err_integral);
// 	if(stop==1)Speed_pid->err_integral=0,stop=0;//清零积分量
	
// 	Speed_pid->PID_OUT=(int32_t)(Speed_pid->kp*Speed_pid->err+Speed_pid->ki*Speed_pid->err_integral);
// }
